#define _CRT_SECURE_NO_WARNINGS

#ifndef ACT_D455_H_
#define ACT_D455_H_

#include <iostream>
#include <fstream>
#include <mutex>
#include <thread>
#include <sstream>
#include <math.h>
#include <time.h>
#include <numeric>
#include <cmath>
#include <sys/time.h> 

#include <string>
#include <opencv2/opencv.hpp>
#include <opencv2/dnn.hpp>
#include <time.h>

#include <librealsense2/rs.hpp>
#include <librealsense2/rsutil.h>
#include <librealsense2/hpp/rs_sensor.hpp>
#include <librealsense2/rs_advanced_mode.hpp>
#include <librealsense2/rs_advanced_mode.h>
#include <eigen3/Eigen/Dense>
#include <queue>
#include <unistd.h>
#include <pcl/point_types.h>
#include <pcl/pcl_base.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/normal_3d_omp.h>
// #include <pcl/gpu/features/features.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/normal_3d.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>

// #include <pybind11/pybind11.h>
// #include <pybind11/embed.h> 
// #undef max//放在<pybind11/numpy.h>之前（#define NOMINMAX有时不起作用）
// #include <pybind11/numpy.h>

// extern "C"
// {
// 	#include "process_comm.h"
// }


#define DEBUG        	//测试员模式
//  #define RECORD_VIDEO	//过程录像
#define CAPTURE
#define TXT 			//运行数据记录
// #define TIME			//程序计时
#define IFCAMERA		//已连接相机
#define COMMUN			//进程通信

// #define PCL
#define CLOUD_GENERATE_THRESHOULD 100	//点云下采样的阈值

#define IMAGE_WIDTH 	640
#define IMAGE_HEIGHT 	480
#define DEPTH_WIDTH 	640
#define DEPTH_HEIGHT 	480

#define LOAD "/home/action/WORKSPACE/ws06_D455_volleyball/video/20240422_2.bag"

#define BALLRADIUS 0.12 //排球半径（单位：m）

using namespace std;
using namespace cv;
using namespace rs2;
using namespace rs400;
using namespace Eigen;
// namespace py = pybind11;
// using namespace py::literals;
typedef pcl::PointXYZ 			PointType;
typedef pcl::PointCloud<PointType> 	PointCloud;
typedef PointCloud::Ptr 		pPointCloud;

//-- ROI of an object
typedef struct
{
	double xMin;
	double xMax;

	double yMin;
	double yMax;

	double zMin;
	double zMax;

} ObjectROI;

class ActD455
{
public:
	ActD455();
	ActD455(const ActD455&) = delete;
	ActD455 operator=(const ActD455&) = delete;
	~ActD455();

	void Init(void);												//初始化
	void Update(void);												//更新图像
	inline cv::Mat GetSrcImage(void)const{return srcImage;}				//获得彩色源图像												
	inline cv::Mat GetDepthImage(void)const{return depthImage;}			//获得深度图像
	inline uint16_t* GetData(void)const{return data;}
	void GetCameraParam(rs2_intrinsics& _color_intrin, rs2_intrinsics& _depth_intrin, rs2_extrinsics& _depth2color_extrin, 
						rs2_extrinsics& _color2depth_extrin, uint16_t* _data);
	void release(void);
	Point3f getPointFromPixel(Point2f Point2d);
	Point3f getPointFromPixel(Point2f Point2d, float depth);
	double averange_distance(int i,int j);
	float depth_correct_Delta(float distance);							//深度中心测距误差补偿
	float depth_correct_2(float distance);							//深度中心测距误差补偿

	std::tuple<uint8_t, uint8_t, uint8_t> GetColorTexture(rs2::video_frame texture, rs2::texture_coordinate Texture_XY);

	char cameraFlag = false;
	//深度误差补偿参数，error = A*dis**2 + B*dis + C
	float corrcetA_1 = 0.00955916;
	float corrcetB_1 = -0.00675201;  
	float corrcetC_1 = 0.00731094;
	float corrcetA_2 = 0.02038193;
	float corrcetB_2 = -0.02376775;
	float corrcetC_2 = 0.01914176;


	//截图
	int picNum = 45;			//截图的起始数
	string picName;				//设置截图名字
	uint16_t* data;

public:
	pPointCloud GetSrcCloud(void) {return pointCloud;}
	bool PointCloudGenerate(cv::Rect roiRect);
	PointType GetCenterPoint(void) {return center_point;}
private:
	pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud; // 根据roi区域取出的点云
	PointType center_point;							// roi中心区域的点坐标

private:
	rs2::context ctx;
	size_t device_count;	
	rs2_intrinsics color_intrin;			//颜色相机内参
	rs2_intrinsics depth_intrin;			//深度相机内参
	rs2_extrinsics depth2color_extrin;		//深度向颜色外参
	rs2_extrinsics color2depth_extrin;
	rs2::pointcloud  rs2Cloud;				//深度图计算得到的点云 RS2指realsense2系列相机
	rs2::points      rs2Points;				//点云格式的点
	rs2::align       align;					//对齐图像
	rs2::pipeline    pipe;					//数据传输管道
	rs2::config      cfg;					//初始化
	rs2::frameset    frameSet;				//帧设置
	cv::Mat 		srcImage;				//源图片，彩色图片source image
	cv::Mat         depthImage;				//深度图


	rs2::temporal_filter tem_filter;		//时间过滤器
	rs2::spatial_filter spat_filter;		//
};
#endif
